#include "CollectionDebugMsg.h"
#include "printf.h"

module ExtDebugLogP {
    provides {
        interface CollectionDebug;
        interface SplitControl;
    }
    uses {
        //interface Boot;
        
        interface Pool<CollectionDebugMsg>;
        interface Queue<CollectionDebugMsg*>;
        
        interface AMSend as UARTSend;
        interface Receive as UARTReceive;
        
        interface LogRead;
        interface LogWrite;
        interface Leds;
        interface Timer<TMilli> as DebugTimer;
        interface Timer<TMilli> as DelayTimer;
    }
} 
implementation {
	  message_t uartMessage;
	  CollectionDebugMsg debugMessage;
	  
	  bool uartbusy;
	  bool flashbusy;
	  bool flashinit;
	  bool readdone;
	  
	  bool flashfail;
    
    uint16_t statLogReceived = 0;
    uint16_t statEnqueueFail = 0;
    uint16_t statAppendFail=0;//statSendFail = 0;
    uint16_t statAppendDoneFail = 0;
    uint16_t statAppendDoneOk = 0;
    uint16_t statAppendDoneBug = 0;

    command void CollectionDebug.print() {
//pr("%d %d %d %d %d %d\n", statAppendFail,statLogReceived,statEnqueueFail,
//                 statAppendDoneOk,statAppendDoneFail,statAppendDoneBug);
    }

    command error_t SplitControl.start() {
        //flashbusy = TRUE;
        flashbusy = FALSE;
        uartbusy = FALSE;

        statAppendFail = 0;
        statLogReceived = 0;
        statEnqueueFail = 0;
        statAppendDoneOk = 0;
        statAppendDoneFail = 0;
        statAppendDoneBug = 0;

        flashinit = FALSE;
        readdone = FALSE;
        flashfail = FALSE;
        
        //call LogRead.seek(SEEK_BEGINNING);
				
				//call LogWrite.sync();
				
        signal SplitControl.startDone(SUCCESS);

				//return SUCCESS;
	return call LogWrite.erase();
    }
    command error_t SplitControl.stop() {}

    task void flashEchoTask() {
        if (flashbusy) {
            return;
        } else if (call Queue.empty()) {
            return;
        } else {
            CollectionDebugMsg* smsg = call Queue.head();
            error_t eval = call LogWrite.append(smsg, sizeof(CollectionDebugMsg));
            if (eval == SUCCESS) {
                flashbusy = TRUE;
                return;
            } else {
                //Drop this packet? Don't retry.
                statAppendFail++;
                call Queue.dequeue();
                call Pool.put(smsg);
                if (! call Queue.empty())
                    post flashEchoTask();
            }
        }
    }
    
    task void readEventTask() {
    	error_t eval;
    	if (readdone) return;
    	
    	eval = call LogRead.read(&debugMessage, sizeof(CollectionDebugMsg));
    	if (eval == SUCCESS) 
    		flashbusy = TRUE;
    	else
    	  post readEventTask(); // if not readdone, and failed, repost the task!!
    }

    event void LogWrite.appendDone(void *msg, storage_len_t len, bool lost, error_t error) {
        CollectionDebugMsg* qh = call Queue.head();
        if (qh == NULL || qh != msg) {
            //bad mojo
            statAppendDoneBug++;
        } else {
            call Queue.dequeue();
            call Pool.put(msg);  
            if (error == SUCCESS) 
                statAppendDoneOk++;
            else 
                statAppendDoneFail++;
        }
        flashbusy = FALSE;
        if (!call Queue.empty()) 
            post flashEchoTask();
    }
    
    event void LogRead.readDone(void* msg, storage_len_t len, error_t err) {
    	CollectionDebugMsg* dbg_msg;
    	flashbusy = FALSE;
    	  
    	if (msg != &debugMessage) return;
      if (uartbusy) return;
      
      if (err != SUCCESS) {
        return;
      }
      if (len != sizeof(CollectionDebugMsg) && len != 0) {
        return;
      }
      
      if (err == SUCCESS && len == 0) {
      	readdone = TRUE;
      	call LogRead.seek(SEEK_BEGINNING);
      }
           
      dbg_msg = (CollectionDebugMsg*)call UARTSend.getPayload(&uartMessage, sizeof(CollectionDebugMsg));
    	memcpy(dbg_msg, msg, sizeof(CollectionDebugMsg));

    	if (call UARTSend.send(0xffff, &uartMessage, sizeof(CollectionDebugMsg)) == SUCCESS) {
        //pr("UARTsend success\n");
        uartbusy = TRUE;
      }
    }
    
    event void UARTSend.sendDone(message_t* msg, error_t err) {
    	uartbusy = FALSE;
    	// next one
    	if (!readdone)
    	  post readEventTask();
    }

    event message_t* UARTReceive.receive(message_t* msg, void* payload, uint8_t len) {
      uint8_t type = *((uint8_t*)payload);
      if (type == DEBUG_EXTLOG_READ) {
        call DelayTimer.startOneShot(10*1024L);
        call CollectionDebug.logEvent(DEBUG_AM_SERIAL);
      }
      return msg;
    }

    event void DelayTimer.fired() {
      call CollectionDebug.readEvent();
    }
    
    command error_t CollectionDebug.readEvent() {
    	if (flashbusy)
    	  return EBUSY; // the flash is busy
    	
    	readdone = FALSE; 
    	//call LogRead.seek(SEEK_BEGINNING);
      post readEventTask();
      //pr("readEvent\n");
    	
    	return SUCCESS;
    }
    
   command error_t CollectionDebug.logEvent(uint8_t type) {
   /*
        //pr ("logEvent\n");
        if (flashfail) return FAIL;
//        if (flashinit) return EBUSY;
        statLogReceived++;
        if (call Pool.empty()) {
        	  post flashEchoTask();
            return FAIL;
        } else {
            CollectionDebugMsg* dbg_msg = call Pool.get();
            	          
	          if (dbg_msg == NULL) {
	            return FAIL;
	          }
	    
            memset(dbg_msg, 0, sizeof(CollectionDebugMsg));

            dbg_msg->type = type;
            //dbg_msg->ts = call DebugTimer.getNow();
            dbg_msg->ts = 0xEEEE;
            dbg_msg->seqno = statLogReceived;

            if (call Queue.enqueue(dbg_msg) == SUCCESS) {
                post flashEchoTask();
                return SUCCESS;
            } else {
                statEnqueueFail++;
                call Pool.put(dbg_msg);
                return FAIL;
            }
        } 
        */
 return SUCCESS;
   }
    /* Used for FE_SENT_MSG, FE_RCV_MSG, FE_FWD_MSG, FE_DST_MSG */
    command error_t TRUSTEDBLOCK CollectionDebug.logEventMsg(uint8_t type, uint16_t msg_id, am_addr_t origin, am_addr_t node) {
      /* if (flashfail) return FAIL;
//    	  if (flashinit) return EBUSY;
        statLogReceived++;
        if (call Pool.empty()) {
        	  post flashEchoTask();
            return FAIL;
        } else {
            CollectionDebugMsg* dbg_msg = call Pool.get();
            
	          if (dbg_msg == NULL) {
	            return FAIL;
	          }
            memset(dbg_msg, 0, sizeof(CollectionDebugMsg));

            dbg_msg->type = type;
            dbg_msg->ts = call DebugTimer.getNow();
            dbg_msg->data.msg.msg_uid = msg_id;
            dbg_msg->data.msg.origin = origin;
            dbg_msg->data.msg.other_node = node;
            dbg_msg->seqno = statLogReceived;

            if (call Queue.enqueue(dbg_msg) == SUCCESS) {
                post flashEchoTask();
                return SUCCESS;
            } else {
                statEnqueueFail++;
                call Pool.put(dbg_msg);
                return FAIL;
            }
 return SUCCESS;
       }*/
          return SUCCESS;
    }
    /* Used for TREE_NEW_PARENT, TREE_ROUTE_INFO */
    command error_t TRUSTEDBLOCK CollectionDebug.logEventRoute(uint8_t type, am_addr_t parent, uint8_t hopcount, uint16_t metric) {
    /*    if (flashfail) return FAIL;
//        if (flashinit) return EBUSY;
        statLogReceived++;
        if (call Pool.empty()) {
        	  post flashEchoTask();
            return FAIL;
        } else {
            CollectionDebugMsg* dbg_msg = call Pool.get();
            
	          if (dbg_msg == NULL) {
	            return FAIL;
	          }
            memset(dbg_msg, 0, sizeof(CollectionDebugMsg));

            dbg_msg->type = type;
            dbg_msg->ts = call DebugTimer.getNow();
            dbg_msg->data.route_info.parent = parent;
            dbg_msg->data.route_info.hopcount = hopcount;
            dbg_msg->data.route_info.metric = metric;
            dbg_msg->seqno = statLogReceived;

            if (call Queue.enqueue(dbg_msg) == SUCCESS) {
                post flashEchoTask();
                return SUCCESS;
            } else {
                statEnqueueFail++;
                call Pool.put(dbg_msg);
                return FAIL;
            }
        }*/
  return SUCCESS;
  }
    /* Used for DBG_1 */ 
    command error_t CollectionDebug.logEventSimple(uint8_t type, uint16_t arg) {
     /*   if (flashfail) return FAIL;
//        if (flashinit) return EBUSY;
        statLogReceived++;
        if (call Pool.empty()) {
        	  post flashEchoTask();
            return FAIL;
        } else {
            CollectionDebugMsg* dbg_msg = call Pool.get();
            
	          if (dbg_msg == NULL) {
	            return FAIL;
	          }
            memset(dbg_msg, 0, sizeof(CollectionDebugMsg));

            dbg_msg->type = type;
            //dbg_msg->ts = call DebugTimer.getNow();
			dbg_msg->ts = 0xEEEE;
            dbg_msg->data.arg = arg;
            dbg_msg->seqno = statLogReceived;

            if (call Queue.enqueue(dbg_msg) == SUCCESS) {
                post flashEchoTask();
                return SUCCESS;
            } else {
                statEnqueueFail++;
                call Pool.put(dbg_msg);
                return FAIL;
            }
        }*/
   return SUCCESS;
 }
    /* Used for DBG_2, DBG_3 */
    command TRUSTEDBLOCK error_t CollectionDebug.logEventDbg(uint8_t type, uint16_t arg1, uint16_t arg2, uint16_t arg3) {
     /*   if (flashfail) return FAIL;
//        if (flashinit) return EBUSY;
        statLogReceived++;
        if (call Pool.empty()) {
        	  post flashEchoTask();
            return FAIL;
        } else {
            CollectionDebugMsg* dbg_msg = call Pool.get();
            
	          if (dbg_msg == NULL) {
	            return FAIL;
	          }
            memset(dbg_msg, 0, sizeof(CollectionDebugMsg));

            dbg_msg->type = type;
            dbg_msg->ts = 0xEEEE;
            //dbg_msg->ts = call DebugTimer.getNow();
            dbg_msg->data.dbg.a = arg1;
            dbg_msg->data.dbg.b = arg2;
            dbg_msg->data.dbg.c = arg3;
            dbg_msg->seqno = statLogReceived;

            if (call Queue.enqueue(dbg_msg) == SUCCESS) {
                post flashEchoTask();
                return SUCCESS;
            } else {
                statEnqueueFail++;
                call Pool.put(dbg_msg);
                return FAIL;
            }
        }
        */
    return SUCCESS;
}


    /* Used for flooding */
    command TRUSTEDBLOCK error_t CollectionDebug.logEventFL(uint8_t type, uint8_t rnd, uint16_t nodeID, uint32_t beginTime, uint32_t finishTime, uint16_t sendNum, uint16_t rcvdNum) {
        if (flashfail) return FAIL;
//        if (flashinit) return EBUSY;
        statLogReceived++;
        if (call Pool.empty()) {
        	  post flashEchoTask();
            return FAIL;
        } else {
            CollectionDebugMsg* dbg_msg = call Pool.get();
            
	          if (dbg_msg == NULL) {
	            return FAIL;
	          }
            memset(dbg_msg, 0, sizeof(CollectionDebugMsg));

            dbg_msg->type = type;
            //dbg_msg->ts = call DebugTimer.getNow();
            //dbg_msg->ts = 0xEEEE;
            dbg_msg->rnd = rnd;
            dbg_msg->nodeID = nodeID;
            dbg_msg->beginTime = beginTime;
            dbg_msg->finishTime = finishTime;
            dbg_msg->sendNum = sendNum;
            dbg_msg->rcvdNum = rcvdNum;
            
            dbg_msg->seqno = statLogReceived;

            if (call Queue.enqueue(dbg_msg) == SUCCESS) {
                post flashEchoTask();
                return SUCCESS;
            } else {
                statEnqueueFail++;
                call Pool.put(dbg_msg);
                return FAIL;
            }
        }
        
    return SUCCESS;
}



   command error_t CollectionDebug.logEventTR(am_addr_t origin, uint16_t origin_seqno, uint8_t type, uint8_t SorR, am_addr_t parent, uint32_t time, uint8_t rssi)
{
/*
	if (flashfail) return FAIL;
//        if (flashinit) return EBUSY;
        statLogReceived++;
        if (call Pool.empty()) {
        	  post flashEchoTask();
            return FAIL;
        } else {
            CollectionDebugMsg* dbg_msg = call Pool.get();
            
	          if (dbg_msg == NULL) {
	            return FAIL;
	          }
            memset(dbg_msg, 0, sizeof(CollectionDebugMsg));

            dbg_msg->origin = origin;
            dbg_msg->origin_seqno = origin_seqno;
            dbg_msg->type = type;
            dbg_msg->SorR = SorR;
            dbg_msg->parent = parent;
            dbg_msg->time = time;
            dbg_msg->rssi = rssi;
            dbg_msg->seqno = statLogReceived;
            if (call Queue.enqueue(dbg_msg) == SUCCESS) {
                post flashEchoTask();
                return SUCCESS;
            } else {
                statEnqueueFail++;
                call Pool.put(dbg_msg);
                return FAIL;
            }
        }
        */
}
 
    
  //event void LogRead.readDone(void* msg, storage_len_t len, error_t err) {}
  //event void LogWrite.appendDone(void* msg, storage_len_t len, bool lost, error_t err) {}
    event void DebugTimer.fired() {}

    event void LogWrite.eraseDone(error_t result) {
      if (result != SUCCESS) {
        flashfail = TRUE;
        call Leds.led0On();
      } else {
        call Leds.led1On();
      }
      
      flashbusy = FALSE;
      flashinit = FALSE;
      //call LogWrite.sync();
      signal SplitControl.startDone(result);
    }
    event void LogWrite.syncDone(error_t result) {
    	//if (result == SUCCESS) {
        //call Leds.led2On();
      //}
    }
    event void LogRead.seekDone(error_t error) {}
}

